#!/usr/bin/env python3
# coding=utf-8
"""
  此代码：通过不断获取磁力计角度，
          通过两点GPS的经纬度计算出两点的距离和目标方位角
          通过磁力计角度和目标方位角和距离判断需要让小车进行相应的控制操作
          
  此代码是全部上位机进行获取数据，底层只需要执行驱动  
  此代码用到了 gps_server5.py
              USART.py
              GPS_SEND.py
              data_plan.py
"""

import base64
import serial
import datetime

import rospy
import socket
import struct
from std_msgs.msg import Float32MultiArray
import multiprocessing
import GPS_SEND
import USART 
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
import time
import threading
import sys
import data_plan
# 定义服务器的IP和端口（可根据实际情况调整）
host = '192.168.195.111'
port = 6666

# 全局变量用于存储接收到的数据（经度、纬度等）
lat_lon1 = [0, 0, 0, 0]
lat_lon2 = [0, 0, 0, 0]
get_check1 = 0.0  # 校验位1（888.0有效）
get_speed = 0.0  # 速度
get_mode = 0.0  # 飞行模式/任务：1.0表示...2.0表示...
get_doit = 0.0  # 执行确认位(1.0表示有效，0.0表示无效)
get_longitude = [0.0, 0.0, 0.0, 0.0]  # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
get_latitude = [0.0, 0.0, 0.0, 0.0]  # 列表存储纬度
get_altitude = [0.0, 0.0, 0.0, 0.0]  # 列表存储海拔（高度）
get_check2 = 0.0  # 校验位（666.0有效）

send_check1 = 888.0
send_speed = 0.0
send_mode = 0.0  # 飞行模式/任务：1.0表示...2.0表示...
send_doit = 1.0  # 执行确认位(1.0表示有效，0.0表示无效)
send_longitude = [0.0, 0.0, 0.0, 0.0]  # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
send_latitude = [0.0, 0.0, 0.0, 0.0]  # 列表存储纬度
send_altitude = [0.0, 0.0, 0.0, 0.0]  # 列表存储海拔（高度）
send_check2 = 666.0

go_xaryy= [0xFF,0x01,0,2,0x01,0xAA]#发送前进的数组数据

turn_left_xarry=[0xFF,0x01,0,2,0x03,0xAA]#发送左转的数组数据

turn_right_xarry = [0xFF,0x01,0,2,0x04,0xAA]#发送右转的数组数据

stop_xarry=[0xFF,0x01,0,0,0x05,0xAA]#发送停止的数组数据

update_angle_xarry=[0xFF,0x21,0,0,0,0xAA]#发送角度重新获取的数组数据

angle_flag = 0#角度标志位，1表示没有对准，0表示对准

send_data_flag = 0

#flag =0 #flag标志位，只需要进入一次hadle_lat_lon函数，只需要发送一次目标点

yaw_value = 0.0 #磁力计imu的角度（顺时针为 0 ~ -180，逆时针为 0 ~ 180）
angle = 0.0 #接收下位机计算目标点相对于起始点的方位角是多少。
distance = 0.0 #计算出两点GPS的距离值
lat_air = 0.0#无人机纬度
lon_air = 0.0#无人机经度
lat_car = 0.0#无人车纬度
lon_car = 0.0#无人车经度
flag_cntrl=0


#------------------服务端口用于接收无人机的经纬度
def start_server():
    global get_longitude, get_latitude, get_altitude, lat_long
    # 创建 TCP socket
    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    try:
        # 绑定 IP 和端口
        server_socket.bind((host, port))

        # 开始监听连接
        server_socket.listen(5)  # 最多允许5个待连接的客户端
        rospy.loginfo("服务器启动，监听 %s:%s...", host, port)

        while not rospy.is_shutdown():
            # 接受客户端连接
            client_socket, addr = server_socket.accept()
            rospy.loginfo("连接来自 %s", addr)

            while not rospy.is_shutdown():
                # 接收 17 个 double 的数据，总大小为 136 字节
                data = client_socket.recv(136)  # 每个 double 是 8 字节
                if not data:
                    rospy.loginfo("客户端 %s 已断开连接", addr)
                    break  # 退出内层循环，等待下一个客户端

                # 解包接收到的数据
                try:
                    unpacked_data = struct.unpack('>17d', data)
                    get_check1, get_speed, get_mode, get_doit, \
                    get_longitude[0], get_latitude[0], get_altitude[0], \
                    get_longitude[1], get_latitude[1], get_altitude[1], \
                    get_longitude[2], get_latitude[2], get_altitude[2], \
                    get_longitude[3], get_latitude[3], get_altitude[3], get_check2 = unpacked_data

                    rospy.loginfo("收到来自 %s 的数据:", addr)
                    rospy.loginfo("  经度: %s", get_longitude[0])
                    rospy.loginfo("  纬度: %s", get_latitude[0])
                except struct.error as e:
                    rospy.logerr("解包数据时发生错误: %s", e)

                if get_check1 == 888.0 and get_check2 == 666.0:
                    send_check1 = 888.0
                    send_speed = 0.0
                    send_mode = 0.0  # 飞行模式/任务：1.0表示...2.0表示...
                    send_doit = 1.0  # 执行确认位(1.0表示有效，0.0表示无效)
                    send_longitude = [0.0, 0.0, 0.0, 0.0]
                    send_latitude = [0.0, 0.0, 0.0, 0.0]
                    send_altitude = [0.0, 0.0, 0.0, 0.0]
                    send_check2 = 666.0
                    # 获取到经纬度数据就需要，发送确认信息 send_doit=1.0
                    datas_to_send = [send_check1, send_speed,send_mode,send_doit,\
                                send_longitude[0], send_latitude[0],send_altitude[0],\
                                send_longitude[1], send_latitude[1],send_altitude[1],\
                                send_longitude[2], send_latitude[2],send_altitude[2],\
                                send_longitude[3], send_latitude[3],send_altitude[3],send_check2]
                    try:
                        # 使用 struct 打包数据，17个 double 类型
                        packed_data = struct.pack('>17d', *datas_to_send)
                        # 发送打包的数据到服务器
                        client_socket.sendall(packed_data)
                        print(f"Sent data: {datas_to_send}")
                        send_doit = 0.0  # 确认位清零
                    except BlockingIOError:
                        print("发送数据时阻塞，请稍后再试。")
                    except Exception as e:
                        print(f"发送数据时出错: {e}")

                    #获取GPS的经纬度数据通过float_to_int处理
                    # 并保存到两个数组lat,lon,然后进行合并一个数组进行返回出去
                    lat = GPS_SEND.float_to_int(get_latitude[0],lat_lon1)
                    lon = GPS_SEND.float_to_int(get_longitude[0],lat_lon2)
                    #print(lat)
                    #print(lon)
                    lat_and_lon = lat + lon
                    #print(lat_and_lon)
                    client_socket.close()
                    server_socket.close()
                return lat_and_lon


            # 关闭当前客户端连接
            client_socket.close()
            rospy.loginfo("关闭与 %s 的连接，准备接收下一个连接...", addr)

    except socket.error as e:
        rospy.logerr("Socket 相关错误: %s", e)
    except KeyboardInterrupt:
        rospy.loginfo("\n服务器正在关闭...")
    finally:
        server_socket.close()
        client_socket.close()
        rospy.loginfo("服务器已关闭。")


# ---------------IMU 回调函数(获取方向航向角)
def imu_callback(msg):
    global yaw_value
    if not rospy.is_shutdown():
      if msg.orientation_covariance[0] < 0:
          return
      # 四元数转成欧拉角
      quaternion = [
          msg.orientation.x,
          msg.orientation.y,
          msg.orientation.z,
          msg.orientation.w
      ]
      (roll,pitch,yaw) = euler_from_quaternion(quaternion)
      # 弧度换算成角度
      #yaw = yaw*180/math.pi
      yaw = yaw*180/3.14159265 -35
      yaw_value = -yaw 
      rospy.loginfo("朝向= %.0f", -yaw)



#-----------------根据方位角误差控制小车运动
def chuli():
    global sender
    global angle,yaw_value
    global angle_flag
    global angle_num 
    #angle_diff = jisuan_angle(angle,yaw_value)
    
    angle_diff = angle - yaw_value
    #print(angle)
    #print(yaw_value)
    # 处理角度差，使其在 [-180, 180] 范围内
    if angle_diff > 180:
        angle_diff -= 360
    elif angle_diff < -180:
        angle_diff += 360
    print("计算的差值angle_diff")
    print(angle_diff)
    
    # 判断旋转方向
    if not rospy.is_shutdown():
      if angle_diff < angle_num and angle_diff > -angle_num and angle_flag == 0:    
          #停止
          sender.air_speed(stop_xarry)
          #print("停止")
          time.sleep(1)
          #sender.air_speed(go_xaryy)
          print("前进")
          angle_flag =1
      elif angle_diff > 0 and angle_flag == 0:
          # 顺时针旋转
          sender.air_speed(turn_right_xarry)
          #print("右转")
      elif angle_diff < 0 and angle_flag == 0:
          # 逆时针旋转
          sender.air_speed(turn_left_xarry)
          #print("左转")
      elif (angle_diff > angle_num or angle_diff < -angle_num) and angle_flag == 1:
          #如果超出范围我就需要发送停止
          sender.air_speed(stop_xarry)
          #print("超出范围了！！！！")
          time.sleep(0.5)
          #然后需要更新当前计算好的角度值，在进行下一次运动判断
          angle_flag =0
          time.sleep(0.5)


#线程1-----------------task1 向基站发送 GGA 格式的 GPS 数据，并接收服务器返回的RTCM数据，将这些数据转发给RTK模块
def task1():
    global serport
    while not rospy.is_shutdown():
        global GGA_STRING
        sock.send(GGA_STRING.encode())
        #print("\r\n\r\nSend GGA to NTRIP caster:%s" % GGA_STRING)
        response = sock.recv(4096)
        #print("Receive RTCM data from server:%d bytes.(should send the RTCM data to GNSS RTK module)" % len(response))	
        hex_string = ''.join(format(b,'02x') for b in response).upper()
        #print(hex_string)
        serport.write(response)
        time.sleep(1)

#线程2-----------------task2 读取RTK数据并打印出来 ，还有计算距离和方位角 
def task2():
    global serport
    global lat_car
    global lon_car
    global lat_air
    global lon_air
    global distance
    global angle

    while not rospy.is_shutdown():
       global GGA_STRING
       rxData =serport.readline() #读取里面的内容
       if rxData.find(b'$GNGGA')!=-1:#
           GGA_STRING=rxData.decode('utf-8')
           line = str(rxData.decode('utf-8')).split(',')  # 将line以“，”为分隔符
       if line[3]=='N':#dingwei is ok
           #纬度
           lat_car = float(line[2][:2]) + float(line[2][2:])/60
            # 读取第5个字符串信息，从0-3为经度，即经度为117，再加上后面的一串除60将分转化为度
            #经度
           lon_car = float(line[4][:3]) + float(line[4][3:])/60
           #print("latitude:",'{:.6f}'.format(lat_car))
           #print("longitude:",'{:.6f}'.format(lon_car))
           
            #计算出距离的距离和方位角
           distance =data_plan.get_distance(lat_air,lon_air,lat_car,lon_car)
           #angle = data_plan.get_angle(lat_air,lon_air,lat_car,lon_car)
           angle = data_plan.get_angle(lat_car, lon_car, lat_air, lon_air)
           print("distance:",distance)
           print("angle:",angle)

           current_time = datetime.datetime.now()
           #print("当前时间：", current_time)
       if line[6]=='1':
           print ('GPS DATA VAULE')
       elif line[6]=='2':
           print('DIFF MODE')
       elif line[6]=='4':
           print('RTK FIXD')
       elif line[6]=='5':
           print('RTK FLOAT')
       # str(GGA_STRING,'utf-8')
            #print(GGA_STRING)  


#------------------主程序
if __name__ == "__main__":
    angle_num = 10
    try:
      #------------TK的初始化（设置串口的初始化，设置RTK的初始化，并且连接RTK账户）
      serport = serial.Serial("/dev/ttyUSB0",460800)#端口根据你实际使用的进行更换
      serport.flushInput()
      time.sleep(0.5)#单位是s

      # NTRIP Caster information
      NTRIP_IP = "120.253.239.161"#差分平台的IP地址端口一般都是8002固定，用户名和密码进行修改
      NTRIP_PORT =8001
      NTRIP_USERNAME = "abyp10563"
      NTRIP_PASSWORD ="ucyhe6m0"
      NTRIP_MOUNT_POINT = "RTCM33_GRC"#这个根据不同平台的接入点不一样进行改动，移动的一般就是用这个
      GGA_STRING = "$GNGGA,115713.000,3149.301528,N,11706.920684,E,1,17,0.88,98.7,M,-3.6,M,,*58\r\n"
      # Create a TCP socket and connect to the NTRIP Caster15.
      try:
          sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
          sock.connect((NTRIP_IP, NTRIP_PORT))
          print("Connected.")
      except socket.error as err:
          print("Connect fail,exit.")
          exit(1)
      # Send login request

      Authorization=(NTRIP_USERNAME+ ":" + NTRIP_PASSWORD)
      Authorization = base64.b64encode(Authorization.encode()).decode()
      requestHead = f"""GET /{NTRIP_MOUNT_POINT} HTTP/1.0\r\nUser-Agent: MOZIHAO-GNSS\r\nAccept: */*\r\nConnection: close\r\nAuthorization: Basic {Authorization}\r\n\r\n"""
      sock.send(requestHead.encode())
      print("send auth msg to NTRIP caster:%s" % requestHead)
      time.sleep(0.2)
      # Receive and parse the NTRIP login response
      response = sock.recv(1024)
      print("receive from server:%s" % response.decode( ))
      if "ICY 200 OK" not in response.decode():
          print("login to NTRIP caster fail,exit.")
          exit(1)
      print('login to NTRIP caster successful.')
      # --------------------------------------------------------------------------


      sender = USART.SerialSender("/dev/ttyS4", 115200)
      print("/dev/ttyS4;115200")
      flag =0

      rospy.init_node('gps_server_node', anonymous=True) 
  
      #订阅磁力计航向角
      imu_sub = rospy.Subscriber("/imu",Imu,imu_callback,queue_size=10)
      
      #获取无人机的GPS数据
      lat_and_lon_air = start_server()
      lat_air = float(f"{lat_and_lon_air[0]}.{lat_and_lon_air[1]}{lat_and_lon_air[2]}{lat_and_lon_air[3]}")  #纬度
      lon_air = float(f"{lat_and_lon_air[4]}.{lat_and_lon_air[5]}{lat_and_lon_air[6]}{lat_and_lon_air[7]}")  #经度
    
      #创建线程，计算GPS的角度和距离
      task1_thread1 = threading.Thread(target=task1)
      task2_thread2 = threading.Thread(target=task2)
      task1_thread1.start()
      task2_thread2.start()
      time.sleep(1)
      
      
      time.sleep(9)
      while not rospy.is_shutdown():
          if distance > 3 : 
              if distance <8:
                angle_num = 30
              chuli()
          else:
              #小车在范围之内
              sender.air_speed(stop_xarry)  
              task1_thread1.stop()
              task2_thread2.stop()            
              sys.exit()
              break
          rospy.sleep(0.1)  
     
      rospy.on_shutdown(sender.close_serial)
      
    except rospy.ROSInterruptException:
        pass 
    
        
        
        
